#include "tcv_measure.h"
#include "cv_measure.h"

int RemovePreviousFrame(cv::VideoCapture VideoCapture1,
                        cv::VideoCapture VideoCapture2,
                        int fram_num = 30)
{
    for(int i=0;i<fram_num;i++)
    {
        //1、帧获取
        VideoCapture1 >> rgbImageL;
        VideoCapture2 >> rgbImageR;

        if(rgbImageL.empty()||rgbImageR.empty())
        {
            cout <<"video empty" <<endl;
            return -1;
        }
    }
    return 1;
}


TCV_MeasureHandle::TCV_MeasureHandle(cv::Mat picture_left, cv::Mat picture_right):
    m_pictureLeft(picture_left),
    m_pictureRight(picture_right)
{
    ;
}

int TCV_MeasureHandle::MeasureDistance(int Height,Vec3f &o_distance)
{
    o_distance = 0;
    int return_judge = 0;

    //3.1
    tf_init();

    Mat frame;
    vector<Point> obj;

    //calculate time
    struct timespec tpstart;
    struct timespec tpmiddle;
    struct timespec tpend;
    long timedif;
    clock_gettime(CLOCK_MONOTONIC, &tpstart);

    //1、帧获取
    if(m_pictureLeft.empty()||m_pictureRight.empty())
        return -1;

    frame = rgbImageL.clone();

    //cout << "frame.size "<<frame.size<<endl;

    //2、深度处理
    tf_dis();

    //4、获取水杯位置
    Point p = Point(imageWidth/2-100, Height);
    o_distance = xyz.at<Vec3f>(p);
    cout << "in world coordinate: " << o_distance << endl;

    obj.push_back(p);
    show_dect_point(obj,Point(p.x,p.y),true);
    cout << "p" << p;

    m_measureOutput = frame;
//    imshow("YOLO-Detections", frame);

    clock_gettime(CLOCK_MONOTONIC, &tpend);
    timedif = 1000000 * (tpend.tv_sec - tpstart.tv_sec) + (tpend.tv_nsec - tpstart.tv_nsec) / 1000;
    fprintf(stdout, "it took %ld microseconds\t", timedif);
    timedif = 1000000 * (tpend.tv_sec - tpmiddle.tv_sec) + (tpend.tv_nsec - tpmiddle.tv_nsec) / 1000;
    fprintf(stdout, "process took %ld microseconds\r\n", timedif);

    waitKey(30);

    clock_gettime(CLOCK_MONOTONIC, &tpmiddle);


    //        CloseCarmera();

    //    if(return_judge == 1)
    //        return 1;
    //    else
    //        return return_judge;
    return 1;
}

cv::Mat TCV_MeasureHandle::GetMeasureResult()
{
    return m_measureOutput;
}
